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ADAPTABLE ITERATIVE AND RECURSIVE KALMAN FILTER 

SCHEMES 

Renato Zanetti* 


Nonlinear filters are often very computationally expensive and usually not suitable 
for real-time applications. Real-time navigation algorithms are typically based 
on linear estimators, such as the extended Kalman filter (EKF) and, to a much 
lesser extent, the unscented Kalman filter. The Iterated Kalman filter (IKF) and the 
Recursive Update Filter (RUF) are two algorithms that reduce the consequences 
of the linearization assumption of the EKF by performing N updates for each new 
measurement, where N is the number of recursions, a tuning parameter. This 
paper introduces an adaptable RUF algorithm to calculate N on the go, a similar 
technique can be used for the IKF as well. 


INTRODUCTION 

The Kalman filter [1,2] is an optimal estimation algorithm. The optimality holds in terms of 
minimum mean square error and maximum likelihood estimation under several conditions. These 
conditions are met when all noises and the initial estimation error are Gaussian, and when the dy- 
namics and measurements are linear; under these conditions the Kalman filter is globally optimal. 
A widely used algorithm in real-time nonlinear - estimation is the extended Kalman filter [3] (EKF). 
The EKF is a nonlinear approximation of the Kalman filter which assumes small estimation errors 
and approximates them to first order to calculate their covariance matrix. Like the Kalman filter, 
the EKF is also a linear - estimator but relies on the additional assumption that the first order approx- 
imation is valid. Algorithms exists that relax both the aforementioned assumptions. Filters with a 
polynomial update of arbitrary order have been known since the sixties [4], knowledge of moments 
of the conditional estimation error distribution higher than the second are needed for these updates. 
Gaussian sum and particle filters have been used for nonlinear - problems [5,6]. Techniques exist to 
overcome some of the limitations of the EKF linearization assumption. The Gaussian second order 
filter (GSOF) [7] takes into account second-order terms assuming the error distribution is Gaussian. 
The iterated extended Kalman filter (IEKF) [3] recursively improves the center of the Taylor se- 
ries expansion for a better linearization. The unscented Kalman filter [8] (UKF) is able to retain 
higher-order terms of the Taylor series expansion. Underweighting [9] is an ad hoc technique to 
compensate for the second order effects without actually computing them. The Recursive Update 
Filter (RUF) [10] applies the update gradually and re-linearizes at each recursion, hence avoiding 
linearization problems. 

The number of iterations in the IKF and RUF are user-defined parameters that needs to be selected 
by addressing two conflicting design objectives. On the one hand the number of iterations should be 
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chosen high to improve performance; the more iterations the more often the algorithm re-linearizes 
and the better the non-linearity of the measurement is followed. On the other hand whenever com- 
putational time is of concern it is desirable to reduce the number of iterations. In general, the higher 
the degree of nonlinearity of the measurement, the more iterations are needed. A good indicator of 
the performance of the algorithm is the post-update residual (actual measurement minus estimated 
measurement computed with the updated state). The residual should match its predicted covariance. 
Discrepancies between the two indicate the nonlinear effects arc of concern and more iterations arc 
needed. 

The estimate x provided by the IKF is equivalent to using a Gauss-Newton method [1 1] to mini- 
mize the following nonlinear least-squares problem: 

min J =(x - x”) T (P~) _1 (x - x _ ) + (h(x) - y) T R _1 (h(x) - y). (1) 

X 

Newton methods converge on an interval when the derivative of the function is non-zero in the in- 
terval and the interval is “small” enough, i.e. the higher order effects arc not dominating. When 
the second of these two conditions does not apply the method can overshoot and diverge. Much 
like the IKF, the RUF fails when the derivative is zero (as does the EKF). However, because the 
update is applied gradually, no overshooting occurs. While the IKF is a Gauss-Newton method, the 
RUF closely resembles a gradient method. Line searching techniques [12] can avoid divergence 
of Newton methods, they usually rely on numerical optimization and their complexity is not usu- 
ally suitable for real-time applications. Standard numerical optimization techniques can be used to 
calculate the number of iterations or recursions, but they require to repeatedly calculate the perfor- 
mance index Eq. (1), which requires the inversion of the filter covariance matrix, which can prove 
computationally expensive. 

This paper introduces a novel algorithm to adaptably and autonomously select the number of 
recursions on the go. The algorithm takes advantage of the aforementioned relation between the 
measurement residuals and their covariance. Only the covariance of the residual needs to be in- 
verted, which is usually of a much smaller dimension than the state covariance. 


THE ITERATED KALMAN FILTER 


The purpose of the iterated Kalman filter update [3] is to repeatedly calculate the measurement 
Jacobian each time linearizing about the most recent estimate. The iteration is initialized by choos- 
ing 


X M = x fc 


P M — P fc 


The loop is given by 


H 


k.i — 


<9h 

<9x 


X=Xfc . 


K M = PfcH ^ [H M P-H^ + R 




Xfe,j + 1 = + Kj jfc [y fe - h(x fc) j) - H fcj j(x fc)i ) (x fc - x fc)i 

Pfc,i+i = [I - P;7 

The updated state estimate and covariance are given by 


Xfc = X fci 7V, 


Pfc = P k,N: 


( 2 ) 


( 3 ) 

( 4 ) 

( 5 ) 

( 6 ) 

(V) 
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where N is the number of iterations. The IKF is fundamentally different from the proposed algo- 
rithm because it recursively recalculates the center of the Taylor series expansions and re-applies 
the entire update to both the state and the covariance. Notice that both the covariance and the state 
update, at each iteration always “start” from the prior values and x ; . , this is in contrast with the 
following, incorrect implementation [13] 


Xfc,0 

— , Pfc j0 — P fc 

( 8 ) 


= P k ,iHk,i [H M P fcii Hj. + Rfc ]' 1 

(9) 

x fc,i+l 

= + K i)fe [y fe - h(x fcji )] 

(10) 

P k,i+ 1 

— [I P^.^ 

(11) 


Oh 



Ox 

(12) 


x=x fcji 



= Xfc’AT, Pfc = Pk,N 

(13) 


This last set of equations is clearly wrong because it is equivalent to having N identical but inde- 
pendent measurements. Using these equations the covariance will never converge, but it will keep 
decreasing because the filter “thinks” more information is available at each iteration. 

At least a couple of similar algorithms exist to do line searches in the IKF to insure that the 
performance index in Eq. (1) decreases at each iteration, alternatively the line search coefficient 
can be chosen by minimizing Eq. (1) via a nonlinear scalar parameter optimization problem. The 
iteration is usually terminated when successive iteration produce estimated states that do not differ 
“much”. 

THE RECURSIVE UPDATE FILTER 

This section introduces the recursive update filter as first derived in [10]. The Kalman filter equa- 
tions with correlated measurement and process noise [14] arc needed. Define the cross-covariance 
at time t k between the true (unknown) state x/, : and the zero-mean measurement noise r] k as C k 

Cfc = E { (xfc — x k } , (14) 

where x7 is the a priori estimated state. To derive the equations of this scheme a linear measurement 
y k is first assumed 

Yk = HfcXfc + rj k . (15) 

Choosing a linear unbiased update 

= Xfc + K k(yk - HfcXfc). (16) 

The optimal gain in terms of minimum variance estimation is given by 

Kfc = (P k Hl + C fc ) (HfeP-H^ + R fc + H fe C fc + CjH J)" 1 , (17) 

where P A is the a priori estimation error covariance matrix and R/, is the measurement error co- 
variance matrix. It is assumed throughout this work that all errors and noises are zero mean. The 
updated estimation error covariance is given by 

P k =(I nxn — KfcHfc)P]7 (I nxn ~ K/ c Hfc) T + KfcRfcK^ — 

(Inxn - K k n k )C k Kj - KfcC£(I nxn - K k H k f, (18) 
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where I nxn is the n x n identity matrix, n being the size of the state vector. 

Assume the same measurement is processed twice, after the first update the a posteriori estima- 
tion error is 

= x fc - x« = (I nxn - KfH^e- - K { ^ Vk , (19) 

where eT = x/, : — x7 is the a priori estimation error. The first optimal gain is 



P^H^HfcP^ + Rfc)- 1 . 


The cross-co variance of e 


(i) 

k 


and r] k is given by 


C' 1} = K k R k . 


( 20 ) 


( 21 ) 


The updated covariance is obtained simplifying Eq. (18) to obtain 

Pl 1} = (Inxn - K^H^Pfc - K fc cl 0)T . (22) 

The cross-covariance between the a priori state and the measurement error is denoted as C[" j and 
is assumed to be zero, i.e. the quantities arc uncorrelated and C[°' = C k = O. Eq. (22) is only 
valid when the gain is chosen as the optimal gain, which is not always true in the remaining of this 
section. Eq. (18) is valid for any choice of K^. 

Processing the same measurement again the second optimal gain is obtained by substituting 
Eq. (21) into Eq. (17) and replacing with pj , 1 1 



(Pf’H, 1 


+ cf'HHtpW 


H t r + R* + Hi-C' 1 ’ + C 


(1)T 


Hr 


(23) 


(2) 

the resulting optimal gain K/ is zero since 

Pfc 1} Hjfc + = (I nxn - K«H fc )p-HT - K^R, 

= PfcHT- K ( 1) (H fc p-HT + R fe ) 

= - F k Hfc = O (24) 


This result is to be expected; after the first update all the information from the measurement is 
extracted, therefore processing the same measurement again, no additional update should occur. 

Assume however that the first update is not optimal, only a fraction of the optimal update is 
applied 

K^ 1} = 0.5P^HT(H fc p-HT + R fc )-\ (25) 

with this choice of K^ 1 ; the resulting K^' 1 is not zero. After the first iteration only half of the optimal 

( 2 ) 

update is applied. During the second iteration the full K ; is applied such that the net result after 
both updates is identical to the standard Kalman algorithm. If three iterations arc performed, and 
each iteration updates one third of the total, the first coefficient is 1/3. The second coefficient is 
1/2 because the remaining optimal update is two thirds of the total, the last coefficient is 1. This 
procedure can be expanded to an arbitrary number N of iterations. 
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Table 1. Recursive update, nonlinear measurements 


Cj; 0) = O, pf 
for * = 1 to N 

r(*) dh I 
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)) 
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Kfc } RfcK 


(<)T 

fc 


For the linear measurement case this algorithm is equivalent to the Kalman filter, making the 
iterations redundant. The benefits of this approach are evident however for nonlinear measurements. 
Given a measurement which is a nonlinear function of the state 

yfc = h(xfc) + ri k (26) 

the algorithm for the nonlinear recursive update is given by Table 1 . 

This algorithm is constructed such that, in the linear case, all the steps have the same length, 
which is reasonable when the number of recursions N is fixed and determined a priori . However, 

notice that there is no requirement for -/ k to be chosen as indicated in Table 1 . The sequence 

(i) 

-< k only has two requirements: it must be composed by number between zero and one (extremes 
excluded) for all elements except the last one that must be equal to one. Consider the linear case 
once again and the following coefficients: 

if = [V 2 1/4 1] (27) 

with this choice the first recursion performs half of the update, the second recursion performs one 
fourth of the other half, or one eighth of the total. The last recursion performs all is left, or three 
eighths of the total. Since a linear measurement is assumed, these recursions arc equivalent to a 
single update with the Kalman filter. 

AUTONOMOUS SELECTION OF ITERATIONS 

As mentioned before the IKF is the Gauss-Newton solution to the nonlinear least squares problem 
given by Eq. (1). In the linear measurement case the performance index becomes: 

min J = (x - x“) T (P _ ) _1 (x - x") + (Hx - y) T R _1 (Hx - y). (28) 

X 

Similarly to doing a line search in the IKF, an option is to select the number of steps such that each 
of them guarantees the reduction of the performance index. There arc two issues with this approach. 
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The first is that since the recursive update filter always starts from the previous step, therefore in 
order to calculate the performance index it would be necessary to invert P multiple times, which 
is often a large matrix and usually much larger than the measurement noise or the residual covari- 
ance. The second reason is that choosing an updated state guaranteeing that the performance index 
decreases certainly means a better estimate is found, however it does not guarantee that the state 
update occurs inside a region where the linearization assumption is valid. The IKF drops the lin- 
earization assumption in the state update, however it calculates the covariance still making a first 
order approximation. The goal of this work is to choose the RUF step size such that each small 


update occurs in the linear - region in order to provide a consistent covariance. 

J(x") = (Hr - y) T R“ 1 (Hi' - y). (29) 

evaluating it at the posterior state estimate 

J(k + ) = (x+ - x“) T (P')- 1 (x + - x-) + (Hx+ - y) T R“ 1 (Hx+ - y) (30) 
= e T K T (P")” 1 Ke + (e + ) T R" 1 e + (31) 

where e = y — Hx is the measurement residual and e l: the post-fit residual. In the linear - case 
J(x~) > (7(x + ) and that is given by 

e k = y k - HfcX^ = y k ~ HfcX^ - H k K k (y k - Hpc^T) = (I - H fc K fc )(y fc - H^) (32) 
= (I - H fc P^H£( Hfc P^H£ + R fc )- 1 ))(y fc - H fc x^) (33) 

= R fc (H fc P^H£ + Rfc) -1 (yfc - H fc x^) = K k W^\y k - H fc x^) = R*W* ^ (34) 

where W/, = Fl/ P, FI 1 + R/. therefore the covariance of e f , is given by 

W+ = R fc W^R fe (35) 

and we have that 

(4) T (W,t)- 1 6+ = ejw^e k (36) 

which means that the residual normalized by its standard deviation is the same before and after the 

update. The posterior performance index is therefore given by 

J(x + ) = e T K T (P - ) _1 Ke + (e+) T R- 1 e+ (37) 

= e T W" 1 HP"(P“)“ 1 p-H T W _1 e + (e + ) T W -1 RR _1 RW _1 e (38) 

= e T W 'HP H T W + e T W 1 RW~ 1 e (39) 

= e T W -1 e = (e£) T (W+)" 1 e+ < J(&~) (40) 

Notice that the same relation between prior and posterior residual ratios is also valid in the sub- 
optimal correlated linear - case: 

£ k = y k - HfeX^ = y k - HfcX^ - U k K k (y k - H fe x^ ) = (I - H k K k )(y k - H fc x^ ) (41) 

= (i - yUk(P k Hl + C fc ) (H A: P A 7Hj + Rfc + H fc C fc + C^)- 1 ) (y k - H fe x* ) (42) 
= ((1 - 7 )H fc P A 7HT + g/R/,. + (1 - 7 )H,C fc + 7 Cj H ^)w- 1 e,- = A fc W* ^ (43) 
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where the residual covariance is now W/. = H/,.P, + R& + H/ C/, + CjHj, therefore the 

covariance of is given by 

W+ = (44) 

and we again have that (e£ ) T (W^) _ 1 e^ = eJW^ ] e k . 

In the EKF case we have that 

= y k - h(x+) = y k - h(x~) - H fc K fc (y fc - h(x“)) + 5 = A k (y k - h(x~)) + S (45) 

where S is a vector containing the higher order terms of the Taylor series expansion of the measure- 
ment function. We then have that 

(4) T (W,t)- 1 6+ = ejw^e k + k (46) 

where n is some scalar function of the higher order terms. In the linear case we know in advance 
what the value for the updated normalized residual is, while in the nonlinear case (e^ ) T (W / (' )~ 1 et ~ 
e k W k 1 e k only when the higher order effects of the measurement nonlinearity arc negligible. We 
can take advantage of this knowledge to select the length of of the update step. Furthermore by 
insuring that (e^) T (W^) _ 1 e^ ~ eJ.'W^ k l e k we can also guarantee that the updated state has a 
smaller performance index. The algorithm to adaptably determine the number of recursions N is 
summarized in Table 2 

A few comments arc in order. In Table 2 the positive scalar i) is a design variable indicating the 
amount of nonlinear effects tolerated while selected N. A value of zero means (e£ ) T (W ^!~) _ 1 ej. = 
e k W k 1 e k is strictly enforced. Selecting i) = 0 is a poor choice for nonlinear systems as the 
algorithm will never reach its exit condition. A ceiling to the maximum number of iterations is 
giving by N max , it is a standard practice to assure an algorithm will not loop forever, although 
this condition should never be exercised with an appropriate value of (). This algorithm selects the 
number of recursions based on the length of the very first recursion. All of the successive recursions 
check that the current length is appropriate, if not the length is decreased. Assume for example that 
during the first recursion N = 2 is selected. Therefore right before entering the second recursion 

we set N m i n = N = 2. The second recursion (i = 2) starts with N = 2 and calculates 7 ^ = 1, 

(2) 

however assume the check fails and therefore overwrites N as N = 3, which makes 7 ^ = 0.5. A 

(3) 

third recursion is therefore called with 7 ), ' = 1. 

(i) 

Notice that W! ’ is assumed invertible. This condition is always satisfied so long as R is invert- 
ible. 

NUMERICAL RESULTS 

The example presented here is spacecraft rendezvous as shown in Fig. 1 . The chaser spacecraft 
stalls the approach one hundred meters in front of the target performing a +V-bar approach. The 
estimated state x contains position and velocity, the initial filter covariance Po is obtained setting 
the state uncertainty to 10 m in position and 0.05 m/s in velocity per axis. The dynamics is governed 
by the CW equations 


x = Ax + v 


10 2 I:3x3 03x3 

03x3 0.05 2 T3 x3 


(47) 
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Table 2. Autonomous selection of N, Recursive Update Filter 
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(48) 


with n = 0.001 lrad/s is the target’s orbital rate. The process noise is given by 


E{iz} = 0 E{iziz t } = 


03x3 0 3x3 

o 3x3 io _9 r 


3x3 


(49) 


A range measurement p with 0.1 m accuracy and two hearing angles with 0.1 deg accuracy arc 
available every two seconds. The hearing angles arc azimuth a = tan _1 (x(l)/x(2)) and elevation 



e = sin 1 (x(3)/p). 


y = 


+ V 

E{??} = 0, E {i7?7 T } 


0.1 2 Oix2 

02x1 (0.l7r/180) 2 l2x2 


(50) 

(51) 


One hundred runs are performed in which the initial estimation error and the sensors errors are 
dispersed using zero mean Gaussian independent random variables. Fig. 2 shows the performance 
of the EKF, the lighter lines are the 100 samples of the estimation error, while the thicker black 
lines are the EKF predicted standard deviations. Much like in the previous example the EKF is 
overly optimistic in predicting its performance. Eventually however the EKF is able to recover. 
Under similar but more severe circumstances, it is also possible that the EKF would diverge all 
together [9]. 




Figure 1. Trajectory. 


Fig. 3 shows the performance of the proposed algorithms. It can be seen that the filter outperforms 
the EKF during the first few hundred seconds. The recursive update filter is also able to predict its 
performance as most error samples are within the la prediction and all of them are within the 3 a 
values. The performance of the two algorithms in estimating velocity is very similar to that of the 
position shown in Figs. 2 and 3. The algorithm selects the number of recursions to be equal to one 
for at all times except the very first one. The number of recursions at the first step varies between 3 
and 10 depending on the particular run, as seen in Fig. 4 

CONCLUSIONS 
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Figure 2. EKF Estimation Error and Predicted Covariance. 
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Figure 3. Recursive Update Filter Estimation Error and Predicted Covariance. 
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